import numpy as np
import cv2 as cv
import socket
import DataPrc
<<<<<<< HEAD
from garbage_identify import garbage_identify
from math import pi
from time import sleep
import Arm_Lib
import ArmControl
=======
from math import pi
from time import sleep
import ArmControl
from ArmMove import ArmMover
import rospy, sys
from geometry_msgs.msg import PoseStamped, Pose
import moveit_commander
from moveit_commander import MoveGroupCommander
from copy import deepcopy
import GripperControl
>>>>>>> 7ab8b32aacdca90fbd07e836fad97c25c5aa0f47

count = 0
change_flag = 1
break_flag = 0
beizi_count = 0
HOST = ''
PORT = 8080
move_status = True
msg = [0.0, 0.0, 0.0, 0.0]
lastData = ["0", "0", "0", "0", "0", "0", "0"]
total_list = []
grap_beizi_count = 0
grap_joint = 0.0
# 机械臂摄像头位置初始化
ArmControl.Init_State()
ADDRESS = (HOST, PORT)
# 创建一个套接字
tcpServer = socket.socket(socket.AF_INET, socket.SOCK_STREAM)
# 绑定本地ip
tcpServer.bind(ADDRESS)
# 开始监听
tcpServer.listen(5)

print("等待连接……")
client_socket, client_address = tcpServer.accept()
print("连接成功！")

while True:
    # 按键选择种类
    print("请选择按键")
    key = input()
    key_status = False
    if key == 'q':
        namelist = ["hongzao", "hongtang"]
        key_status = True
    if key == 'w':
        namelist = ["qingju", "puer"]
        key_status = True
    if key == 'e':
        namelist = ["meigui", "chenpi"]
        key_status = True
    if key == 'p':
        break
    grap_count = [0, 0]
    if key_status == True:
        sleep(2)
        while True:
            # 如果夹完杯子则开始夹材料
            if grap_beizi_count == 1:
                DataPrc.OpenLocation(namelist[0])

            # 接收标志数据
            data = client_socket.recv(1024)
            if data:
                # 通知客户端“已收到标志数据，可以发送图像数据”
                client_socket.send(b"ok")
                # 处理标志数据
                flag = data.decode().split(",")
                # 图像字节流数据的总长度
                total = int(flag[0])
                # 接收到的数据计数
                cnt = 0
                # 存放接收到的数据
                data_bytes = b""

                while cnt < total:
                    # 当接收到的数据少于数据总长度时，则循环接收图像数据，直到接收完毕
                    data = client_socket.recv(256000)
                    data_bytes += data
                    cnt += len(data)
                    print("receive:" + str(cnt) + "/" + flag[0])
                # 通知客户端“已经接收完毕，可以开始下一帧图像的传输”
                # client_socket.send(b"ok")
                if count != 15:
                    client_socket.send(b"ok")

                # 数据转化
                print(data_bytes)
                str0 = data_bytes.decode()
                total_list = str0.split(',')
                for i in range(0, len(total_list), 7):
                    list = total_list[i:i + 7]
                    name = list[0]
                    confidence = float(list[1])
                    currentID = int(list[6])

                    # 判断是否为杯子
                    if name == "beizi" and confidence > 0.7 and grap_beizi_count < 10:
                        beizi_count += 1
                        print("未准备就绪")

                    # 判断count
                    if name == namelist[0] and grap_count[0] < DataPrc.DataCount(
                            namelist[0]) and change_flag != currentID:
                        count += 1
                        change_flag = DataPrc.reverse(change_flag)
                        # print(data_bytes)
                        print("未准备就绪")
                    if name == namelist[1] and grap_count[1] < DataPrc.DataCount(
                            namelist[1]) and change_flag != currentID:
                        count += 1
                        change_flag = DataPrc.reverse(change_flag)
                        # print(data_bytes)
                        print("未准备就绪")

                    # 对杯子进行处理
                    if beizi_count == 5:
                        beizi_count = 0
                        msg = DataPrc.DataTransform(int(list[2]), int(list[3]), int(list[4]), int(list[5]))
                        # 杯子数据处理
<<<<<<< HEAD
                        if name == "beizi":
                            msg[3] = 1.5 * msg[3]
=======
>>>>>>> 7ab8b32aacdca90fbd07e836fad97c25c5aa0f47
                        print("msg: ", msg, " name: ", name)
                        if name == "beizi": grap_beizi_count += 1
                        # 机械臂夹取
                        if msg[0] != 0 and move_status == True:
                            move_status = False
<<<<<<< HEAD
                            (joints, grap_joint) = ArmControl.Control(msg, name)
                            print("joints: ", joints)
                            print("grap_joint: ", grap_joint)
                            move_status = True
                            ArmControl.Init_State()
=======
                            ArmControl.Control(msg, name)
                            move_status = True
>>>>>>> 7ab8b32aacdca90fbd07e836fad97c25c5aa0f47
                            sleep(2)

                    # 对材料进行处理
                    if count == 15:
                        count = 0
                        # print("hello!")
                        # 数据处理
                        msg = DataPrc.DataTransform(int(list[2]), int(list[3]), int(list[4]), int(list[5]))
                        print("msg: ", msg)
                        if name == namelist[0]:
                            grap_count[0] += 1
                            print(name)
                        if name == namelist[1]:
                            grap_count[1] += 1
                            print(name)
                        # 机械臂夹取
                        if msg[0] != 0 and move_status == True:
                            move_status = False
<<<<<<< HEAD
                            (joints, grap_joint) = ArmControl.Control(msg, name)
                            print("joints: ", joints)
                            print("grap_joint: ", grap_joint)
                            move_status = True
                            ArmControl.Init_State()
=======
                            ArmControl.Control(msg, name)
                            move_status = True
>>>>>>> 7ab8b32aacdca90fbd07e836fad97c25c5aa0f47
                            sleep(2)

                    # 第一种食材抓取完毕后，关闭第一个抽屉，打开第二个抽屉
                    if grap_count[0] == DataPrc.DataCount(namelist[0]) and grap_count[1] != DataPrc.DataCount(
                            namelist[1]):
                        DataPrc.CloseLocation(namelist[0])
                        DataPrc.OpenLocation(namelist[1])


                    # 两种食材都抓取完毕后，关上第二个抽屉，开始倒水操作
                    if grap_count[0] == DataPrc.DataCount(namelist[0]) and grap_count[1] == DataPrc.DataCount(
                            namelist[1]):
                        break_flag = 1
                        DataPrc.CloseLocation(namelist[1])
                        break

                if break_flag == 1:
                    break_flag = 0
                    # 倒水 操作
<<<<<<< HEAD
                    ArmControl.Pour_Water()
                    # end 操作
                    ArmControl.End_Control(msg)
                    ArmControl.Init_State()
=======
                    ArmControl.Beizi_To_Drink()
                    # end 操作
                    ArmControl.Push_Drink()
                    ArmControl.Get_Drink()
>>>>>>> 7ab8b32aacdca90fbd07e836fad97c25c5aa0f47
                    sleep(2)
                    break


            else:
                print("已断开！")
                break
client_socket.close()









